#include <iostream>
#include <cstdlib>
#include <time.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <libplayerc++/playerc++.h>
#include "args.h"
#define RAYS 32
#include <time.h>
#include <signal.h>
#include <ctime>	
#include <sys/time.h>
#include "ekfblob.cpp"


using namespace std;
using namespace PlayerCc;
FILE *posoutput;
FILE *mapin;
FILE *LOG;


//goto definitions
int N_VIA=5;
int N_giri = 1;
double viapointX[]={3,3,-3,-3,3};
double viapointY[]={1,-2,-2,2,2};
int vp=0;
int ig=1;

bool finish = false;
bool ODOM_ERROR=false;
int debug=0;

int main(int argc, char** argv){

// file di log e output
struct tm *current;
	time_t now;
	time(&now);
	current = localtime(&now);
	char outputdata[50];
	char logFile[50];  	
	sprintf(outputdata,"output_data/loc-%d%d%d%d%d%d.txt",current->tm_year,current->tm_mon,current->tm_mday,current->tm_hour,current->tm_min,current->tm_sec);
sprintf(logFile,"log/LOG-%d%d%d%d%d%d.txt",current->tm_year,current->tm_mon,current->tm_mday,current->tm_hour,current->tm_min,current->tm_sec);

	LOG=fopen(logFile,"w");
	posoutput=fopen(outputdata,"w");


//////////////////// INIZIALIZZAZIONE DI TUTTE LE PERIFERCHE NELL'AMBIENTE - LANDAMARK ATTIVI -

//PlayerClient robot_0(gHostname, 6666); invece che dal robot prendo la posizione dal su stato calcolato con un istanza di ekfblob
PlayerClient robot_1(gHostname, 6667);
PlayerClient robot_2(gHostname, 6668);
PlayerClient robot_3(gHostname, 6669);
PlayerClient robot_4(gHostname, 6670);
//Position2dProxy pp_0(&robot_0, gIndex);	//This is needed in every player client application
Position2dProxy pp_1(&robot_1, gIndex);	//This is needed in every player client application
Position2dProxy pp_2(&robot_2, gIndex);	//This is needed in every player client application
Position2dProxy pp_3(&robot_3, gIndex);	//This is needed in every player client application
Position2dProxy pp_4(&robot_4, gIndex);	//This is needed in every player client application


//////////////////////// INIZIALIZZAZIONE DEI ROBOT ATTIVI 
	
	//These are initialised before the infinite loop
	PlayerClient robot(gHostname, gPort);	//This is needed in every player client application	
	Position2dProxy pp(&robot, gIndex);	//This is needed in every player client application
	LaserProxy lp(&robot, gIndex);		//This is needed in every player client application
	BlobfinderProxy bp(&robot,0);
	ekfblob ekf;

	PlayerClient robot1(gHostname, 6666);	//This is needed in every player client application	
	Position2dProxy pp1(&robot1, gIndex);	//This is needed in every player client application
	LaserProxy lp1(&robot1, gIndex);		//This is needed in every player client application
	BlobfinderProxy bp1(&robot,0);
	ekfblob ekf1;
//////////////////

	//EFK SLAM object

	double x,y,yaw,oldx,oldy,oldyaw;	//Variables used to find change in displacement d_d and change in orientation d_t
	FILE *input;
	input=fopen("input.txt","r");	
	ekf.set(input);
	fclose(input);						//EKF SLAM initialisation

	oldx=ekf.xest.val[0][0];
	oldy=ekf.xest.val[1][0];
	oldyaw=ekf.xest.val[2][0];


	//EFK SLAM object

	double x1,y1,yaw1,oldx1,oldy1,oldyaw1;	//Variables used to find change in displacement d_d and change in orientation d_t
	FILE *input1;
	input1=fopen("input1.txt","r");	
	ekf1.set(input1);
	fclose(input1);		
	oldx1=ekf1.xest.val[0][0];
	oldy1=ekf1.xest.val[1][0];
	oldyaw1=ekf1.xest.val[2][0];

	//file di log - INIZIALIZZAZIONE	
	fprintf(LOG,"  init x ,y, yaw :%4.3f/%4.3f/%3.2f\n",x,y,yaw*180.0/PI);
	printf("  init x ,y, yaw :%4.3f/%4.3f/%3.2f\n",x,y,yaw*180.0/PI);

	int i=0;
	while(!finish){
	//	while (debug!=5){
				debug=debug+1;
		i=i+1;
		
		try{

			//These are called during the inifinite recursive loop
			robot.Read();				//Obtain DR and laser data
			robot1.Read();				//Obtain DR and laser data
						
			x=pp.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
			y=pp.GetYPos();
			yaw=pp.GetYaw();

			x1=pp1.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
			y1=pp1.GetYPos();
			yaw1=pp1.GetYaw();

			//printf("  %d iesimo x ,y, yaw :%4.3f/%4.3f/%3.2f\n",i,x,y,yaw*180.0/PI);
			fprintf(LOG," %d iesimo x ,y, yaw :%4.3f/%4.3f/%3.2f\n",i,x,y,yaw*180.0/PI);

float n;		
if (ODOM_ERROR){
 n=rand() %100;
  n=n*0.00001;
}else {
 n =0;
}

			double d_d=sqrt((oldx-x+n)*(oldx-x+n)+(oldy-y+n)*(oldy-y+n));
			double d_t=yaw-oldyaw;
			normalise(d_t);				//Force all angles to be between +-180 degrees


			double d_d1=sqrt((oldx1-x1+n)*(oldx1-x1+n)+(oldy1-y1+n)*(oldy1-y1+n));
			double d_t1=yaw1-oldyaw1;
			normalise(d_t1);		


			int NBlob=bp.GetCount();	
        		playerc_blobfinder_blob_t ListaBlob[NBlob];		
				printf(" ... ");
			for (int z=0; z<NBlob;z++){
				playerc_blobfinder_blob_t blob=bp.GetBlob(z);
				ListaBlob[z]=blob;
			}

			int NBlob1=bp1.GetCount();	
        		playerc_blobfinder_blob_t ListaBlob1[NBlob1];		
				printf(" ... ");
			for (int z=0; z<NBlob;z++){
				playerc_blobfinder_blob_t blob1=bp1.GetBlob(z);
				ListaBlob1[z]=blob1;
			}
	
			double ltime=bp.GetDataTime();		//Get timestamp for laser
			double ptime=pp.GetDataTime();		//Get timestamp for odometer
			double lpdiff=abs(ltime-ptime);		//Find laser and odometer timestampt difference

			double ltime1=bp1.GetDataTime();		//Get timestamp for laser
			double ptime1=pp1.GetDataTime();		//Get timestamp for odometer
			double lpdiff1=abs(ltime1-ptime1);		//Find laser and odometer timestampt difference

			ekf.predict(d_d,d_t);			//EKF predict using d_d and d_t
			ekf1.predict(d_d1,d_t1);
			//logdata
			fprintf(posoutput,"%f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);
			fprintf(LOG,"stima dello stato - predict: %f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);
			

//getting info from other robots
	matlite ROBOTS;
	int N_ROBOT=6;
        ////////////////
	double xR,yR,tR;

// Robots 
//[0] x cord
//[1] y cord
//[2] t cord
//[3] port

ROBOTS.resize(4,N_ROBOT);

try{
//robot_0.Read();
robot_1.Read();
robot_2.Read();
robot_3.Read();
robot_4.Read();

//xR=pp_0.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
//yR=pp_0.GetYPos();
//tR=pp_0.GetYaw();

//ROBOTS.val[0][0]=xR;
//ROBOTS.val[1][0]=yR;
//ROBOTS.val[2][0]=tR;
//ROBOTS.val[3][0]=robot_0.GetPort();

ROBOTS.val[0][0]=ekf1.xest.val[0][0];
ROBOTS.val[1][0]=ekf1.xest.val[1][0];
ROBOTS.val[2][0]=ekf1.xest.val[2][0];
ROBOTS.val[3][0]=robot1.GetPort();

xR=pp_1.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
yR=pp_1.GetYPos();
tR=pp_1.GetYaw();

ROBOTS.val[0][1]=xR;
ROBOTS.val[1][1]=yR;
ROBOTS.val[2][1]=tR;
ROBOTS.val[3][1]=robot_1.GetPort();

xR=pp_2.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
yR=pp_2.GetYPos();
tR=pp_2.GetYaw();

ROBOTS.val[0][2]=xR;
ROBOTS.val[1][2]=yR;
ROBOTS.val[2][2]=tR;
ROBOTS.val[3][2]=robot_2.GetPort();

xR=pp_3.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
yR=pp_3.GetYPos();
tR=pp_3.GetYaw();

ROBOTS.val[0][3]=xR;
ROBOTS.val[1][3]=yR;
ROBOTS.val[2][3]=tR;
ROBOTS.val[3][3]=robot_3.GetPort();

xR=pp_4.GetXPos();				//Alternatively, you can use v*dt and w*dt if you know the sample time.
yR=pp_4.GetYPos();
tR=pp_4.GetYaw();

ROBOTS.val[0][4]=xR;
ROBOTS.val[1][4]=yR;
ROBOTS.val[2][4]=tR;
ROBOTS.val[3][4]=robot_4.GetPort();


ROBOTS.val[0][5]=ekf.xest.val[0][0];
ROBOTS.val[1][5]=ekf.xest.val[1][0];
ROBOTS.val[2][5]=ekf.xest.val[2][0];
ROBOTS.val[3][5]=robot.GetPort();

//////////////////////
printf("Matrice ROBOTS:\n");
for (int z=0; z<N_ROBOT; z++){
printf("[%f %f %f %f] \n",ROBOTS.val[0][z],ROBOTS.val[1][z],ROBOTS.val[2][z],ROBOTS.val[3][z]);
}
}
		catch (PlayerCc::PlayerError & e)
		{
			std::cerr << e << std::endl;
			return -1;
		}

////////////////////////7

			ekf.update(ListaBlob,NBlob,d_t,lpdiff,ROBOTS);	//EKF update using laser data, d_t and lpdiff
			ekf1.update(ListaBlob1,NBlob1,d_t1,lpdiff1,ROBOTS);	//EKF update using laser data, d_t and lpdiff
			
			fprintf(LOG,"stima dello stato - update: %f %f %f %f %f %f %f %f\n",ekf.xest.val[0][0],ekf.xest.val[1][0],ekf.xest.val[2][0],x,y,yaw,d_d,d_t);

			// goto commands . . . . .  . . . .. . . . .  . . . . 
			if(N_giri!=0)
				if (viapointX[vp] < x+0.2 && viapointX[vp] > x-0.2  && viapointY[vp] < y+0.2 && viapointY[vp] > y-0.2 ){
					vp=vp+1;
					robot.Read();
					//pp.GoTo(viapointX[vp],viapointY[vp],0);
					std::cout << "goto starting, target: " << viapointX[vp]
						<< ", " << viapointY[vp] << std::endl;
					std::cout << "indice target i: " << vp << std::endl;
					std::cout << "giro " << ig << std::endl;
				}

			if(vp>=N_VIA && ig<N_giri){
				ig=ig+1;
				vp=0;
			}

			if(vp>=N_VIA && ig>=N_giri){
				finish= true;
			}

			oldx=x;
			oldy=y;
			oldyaw=yaw;

			oldx1=x1;
			oldy1=y1;
			oldyaw1=yaw1;	

	
		}
		catch (PlayerCc::PlayerError & e)
		{
			std::cerr << e << std::endl;
			return -1;
		}

	}//THE END loop


	fclose(posoutput);
	fclose(LOG);

}




